#include <math.h>
//#include <string.hpp>
#include <stdio.h>

#include <sstream>


#include <iostream>
#include <string>
#include <cstring>



#include <webots/Robot.hpp>


#include <ros/ros.h>
#include <sensor_msgs/Image.h>

#include <CR1/RangeCameraParam.h>

using namespace std;
using namespace webots;

#include "RWRangeCamera.h"


RWRangeCamera::RWRangeCamera()
{

}
RWRangeCamera::~RWRangeCamera()
{

}

void RWRangeCamera::init(webots::Robot* R, ros::NodeHandle* n_in,string name)
{
  n = n_in;
  time_step = R->getBasicTimeStep();
  RANGE = R->getCamera(name);
  RANGE->enable(time_step);
  range_width = RANGE->getWidth();
  range_height = RANGE->getHeight();
  range_min_range = RANGE->getNear();
  range_max_range = RANGE->getMaxRange();

  //ROS PUBLISHER
  std::stringstream RPubname,RFramename;
  RPubname << R->getName() << "/camera/depth/image";
  RFramename << "/webots_kinect_optical_frame";
  pub = n->advertise<sensor_msgs::Image>(RPubname.str(),1000);
  RangeImage.header.frame_id = RFramename.str();
  RangeImage.header.seq = 0;
  RangeImage.height = range_height;
  RangeImage.width = range_width;
  RangeImage.encoding = "32FC1";
  RangeImage.is_bigendian = 0;
  RangeImage.step = range_width *4;
  RangeImage.data.resize(range_height*range_width*4);

  //publish range camera parameters

  param.MaxRange = range_max_range;
  param.horFoV = RANGE->getFov();
  param.verFoV = param.horFoV*range_height/range_width;
  param.phiMin = -param.horFoV/2;
  param.thetaMax = param.verFoV/2;

  param_pub_cont = 0;

  stringstream RPubname2;
  RPubname2 << R->getName() << "/camera/depth/param";
  param_pub = n->advertise<CR1::RangeCameraParam>(RPubname2.str(),1000);

}



void RWRangeCamera::iterate()
{
  const float* range_image = RANGE->getRangeImage();
  RangeImage.header.stamp = ros::Time::now();
  memcpy(RangeImage.data.data(),range_image,range_height*range_width*4);
  pub.publish(RangeImage);

  if (param_pub_cont < 10)
  {
    param_pub.publish(param);
    param_pub_cont++;
  }


}
